﻿#pragma once

#include <Eigen/Dense>
#include <iostream>

using namespace Eigen;

class KalmanFilter
{
private:
    /* data */
    int _stateNum = 0;
    int _measureNum = 0;
    int _controlNum = 0;
    float _T = 0.0;

    MatrixXd _transitionMat;
    MatrixXd _measureMat;
    MatrixXd _controlMat;
    MatrixXd _postCovMat;
    MatrixXd _proNoiseCovMat;
    MatrixXd _meaNoiseCovMat;

    VectorXd _stateVec;
    VectorXd _controlVec;
    VectorXd _processNoise;
    VectorXd _measureNoise;

public:
    KalmanFilter(int stateNum = 0, int measurementNum = 0, int controlNum = 0, float T = 1.0);
    ~KalmanFilter();
    MatrixXd predict();
    MatrixXd correct(const VectorXd& measureVec);
    bool setTansMat(const MatrixXd& transitionMat);
    bool setMeasMat(const MatrixXd& measurementnMat);
    bool setQ(const MatrixXd& proNoiseCovMat);
    bool setR(const MatrixXd& meaNoiseCovMat);
    void info();
};
